#include <string.h>
#include "maxq_config.h"
#include "i2c/maxq_i2c.h"
#include "comm/maxq_comm.h"
#include "compass.h"

/***** Definitions *****/
#define COMPASS_ADDR    (0x42 >> 1)


/****************************************************************************/
int eeprom_read(unsigned int addr, uint8_t * data)
{
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'r';
  msg[1] = addr;
  if (!i2c_master_write(COMPASS_ADDR, 2, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  if (!i2c_master_read(COMPASS_ADDR, 1, msg))
    return 0;

  *data = msg[0];

  return 1;
}

/****************************************************************************/
int ram_read(unsigned int addr, uint8_t * data)
{
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'g';
  msg[1] = addr;
  if (!i2c_master_write(COMPASS_ADDR, 2, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  if (!i2c_master_read(COMPASS_ADDR, 1, msg))
    return 0;

  *data = msg[0];

  return 1;
}

/****************************************************************************/
int opbyte_write(uint8_t data)
{
  uint8_t msg[3];

  msg[0] = 'G';
  msg[1] = 0x74;
  msg[2] = data;
  if (!i2c_master_write(COMPASS_ADDR, 3, msg))
    return 0;

  return 1;
}

/****************************************************************************/
int compass_init(void)
{
  uint8_t data;

  if (!eeprom_read(0, &data))
    return 0;

  data = 'W';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  if (!opbyte_write(0x72))
    return 0;

  data = 'A';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  return 1;
}

/****************************************************************************/
int compass_read(unsigned int * heading)
{
  uint8_t msg[2];

  if (!i2c_master_read(COMPASS_ADDR, 2, msg))
    return 0;

  *heading = msg[0];
  *heading <<= 8;
  *heading += msg[1];

  return 1;
}

